import open3d as o3d
import matplotlib.pyplot as plt

print("Read Redwood dataset")
# 这一步是相机坐标系
color_raw = o3d.io.read_image("00000.jpg")
depth_raw = o3d.io.read_image("00000.png")
# 彩色转化为灰度图
# rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
#     color_raw, depth_raw)
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw,convert_rgb_to_intensity=False)
print(rgbd_image)

plt.subplot(1, 2, 1)
plt.title('Redwood grayscale image')
plt.imshow(rgbd_image.color) # 彩色图  rgbd_image 包含了两张图，深度图表示每个像素的距离，单位毫米
plt.subplot(1, 2, 2)
plt.title('Redwood depth image')
plt.imshow(rgbd_image.depth) # 深度图 正常来讲应该是一张灰度图，但是open3d进行伪色彩化了jet
plt.show()
# # 这一步是 生成点云所使用的坐标系，也叫open3d相机坐标系 仅仅是y 方向改变 已经翻转了y的方向
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
    o3d.camera.PinholeCameraIntrinsic(
        o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))


success = o3d.io.write_point_cloud("copy_dept.pcd",pcd,write_ascii=True, compressed=False,print_progress=True)
"""
这个坐标系进行转换 opencv 也就是相机坐标系 转换成世界坐标系
"""
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
o3d.visualization.draw_geometries([pcd])
